#include <FastSerial.h>
#define IMUPORT Serial1
#define DEBUGPORT Serial
#define BUFFERSIZE 32
FastSerialPort0(DEBUGPORT);
FastSerialPort1(IMUPORT);



//interrupts
#define INT4PIN 2
#define INT5PIN 3
#define INT1PIN 20
#define INT0PIN 21
#define ICP4PIN 49
#define ICPNORM 49
#define INTNORM 108

#define LEDPIN 13

//int1
volatile unsigned int timer1INT1;
volatile unsigned int timer2INT1;
volatile byte flagINT1;
ISR(INT1_vect)
{
  if (EICRA & B00000100) {//if we captured rising
    EICRA= EICRA ^ B00000100;//falling edge
    timer1INT1=TCNT4;
    flagINT1=0;
  }
  else {
    EICRA=EICRA ^ B00000100;//rising edge
    timer2INT1=TCNT4;
    flagINT1=1;
  }
}

//int0
volatile unsigned int timer1INT0;
volatile unsigned int timer2INT0;
volatile byte flagINT0;
ISR(INT0_vect)
{
  if (EICRA & B00000001) {//if we captured rising
    EICRA = EICRA ^ B00000001;//falling edge
    timer1INT0=TCNT4;
    flagINT0=0;
  }
  else {
    EICRA = EICRA ^ B00000001;//rising edge
    timer2INT0=TCNT4;
    flagINT0=1;
  }
}

//int4
volatile unsigned int timer1INT4;
volatile unsigned int timer2INT4;
volatile byte flagINT4;
ISR(INT4_vect)
{
  if (EICRB & B00000001) {//if we captured rising
    EICRB = EICRB ^ B00000001;//falling edge
    timer1INT4=TCNT4;
    flagINT4=0;
  }
  else {
    EICRB = EICRB ^ B00000001;//rising edge
    timer2INT4=TCNT4;
    flagINT4=1;
  }
}

//int5
volatile unsigned int timer1INT5;
volatile unsigned int timer2INT5;
volatile byte flagINT5;
ISR(INT5_vect)
{
  if (EICRB & B00000100) {//if we captured rising
    EICRB = EICRB ^ B00000100;//falling edge
    timer1INT5=TCNT4;
    flagINT5=0;
  }
  else {
    EICRB = EICRB ^ B00000100;//rising edge
    timer2INT5=TCNT4;
    flagINT5=1;
  }
}

//ICP4
volatile unsigned int timer1ICP4;
volatile unsigned int timer2ICP4;
volatile byte flagICP4;
ISR(TIMER4_CAPT_vect)
{
  if (TCCR4B&B01000000) {//if we captured rising
    TCCR4B=TCCR4B ^ B01000000;//falling edge
    timer1ICP4=ICR4;
    flagICP4=0;
  }
  else {
    TCCR4B=TCCR4B ^ B01000000;//rising edge
    timer2ICP4=ICR4;
    flagICP4=1;
  }
}

void setup()
{
  pinMode(LEDPIN, OUTPUT);  //LED debug
  DEBUGPORT.begin(57600);
  IMUPORT.begin(38400);

  //timer4 setup
  TCCR4A=0;
  TCCR4B=B00000001;//prescaler=1
  TCCR4C=0;
  
  //servo setup
  ROLLSERVO.attach(ROLLPIN);
  PITCHSERVO.attach(PITCHPIN);
  PANSERVO.attach(PANPIN);
  TILTSERVO.attach(TILTPIN);
  
  //int1 ISR for tilt signal
  pinMode(INT1PIN, INPUT); //tilt signal from APM
  digitalWrite(INT1PIN,HIGH); //internal pullup
  EICRA|=B00001100;//rising INT1
  EIMSK|=B00000010;//enable INT1

  //int0 ISR for pan signal
  pinMode(INT0PIN, INPUT); //pan signal from APM
  digitalWrite(INT0PIN,HIGH); //internal pullup
  EICRA|=B00000011;//rising INT0
  EIMSK|=B00000001;//enable INT0
  
  //ICP4 ISR for hall effect
  pinMode(ICP4PIN, INPUT); //hall effect
  digitalWrite(ICP4PIN,HIGH); //internal pullup
  TCCR4B|=B11000000;//ICP filter plus rising
  TIMSK4|=B00100000;//ICP enable
  
  //int4 ISR for photointerruptor one
  pinMode(INT4PIN, INPUT); //photointerruptor one signal
  digitalWrite(INT4PIN,HIGH); //internal pullup
  EICRB|=B00000011;//rising INT4
  EIMSK|=B00010000;//enable INT4
  
  //int5 ISR for photointerruptor two
  pinMode(INT5PIN, INPUT); //photointerruptor two signal
  digitalWrite(INT5PIN,HIGH); //internal pullup
  EICRB|=B00001100;//rising INT5
  EIMSK|=B00100000;//enable INT5
  
} 
  
void loop()
{
  
  //Misc variables
  byte i;
  int light=LOW;
  unsigned long t=millis();
  unsigned int INT0val;
  unsigned int INT1val;
  unsigned int INT4val;
  unsigned int INT5val;
  unsigned int ICP4val;
  TILTSERVO.writeMicroseconds(900);
  PANSERVO.writeMicroseconds(900);
  PITCHSERVO.writeMicroseconds(900);
  ROLLSERVO.writeMicroseconds(900);
  
  while(1){
    if (flagINT0) {
      INT0val=timer2INT0-timer1INT0;
      flagINT0=0;   
    }
    if (flagINT1) {
      INT1val=timer2INT1-timer1INT1;
      flagINT1=0;
    }
    if (flagINT4) {
      INT4val=timer2INT4-timer1INT4;
      flagINT4=0;
    }
    if (flagINT5) {
      INT5val=timer2INT5-timer1INT5;
      flagINT5=0;
    }    
    if (flagICP4) {
      ICP4val=timer2ICP4-timer1ICP4;
      flagICP4=0;   
    }

    if( (millis()-t) > 100) { // Main loop runs at 10Hz
      t=millis();
      light=!light;
      digitalWrite(13, light);   // set the LED on
      DEBUGPORT.print(INT0val);DEBUGPORT.print("\t");
      DEBUGPORT.print(INT1val);DEBUGPORT.print("\t");
      DEBUGPORT.print(INT4val);DEBUGPORT.print("\t");
      DEBUGPORT.print(INT5val);DEBUGPORT.print("\t");
      DEBUGPORT.print(ICP4val);DEBUGPORT.print("\t");
      DEBUGPORT.println();
    }//end 10Hz
    
  }//end while(1)
}//end loop()








